function [Status, Scheduled_Targets, PQ] = MSDF_BS_Schedule(PQ, Sensor1_Target_Visit_Order, Sensor2_Target_Visit_Order)
%MSDF_BS_Schedule Summary of this function goes here
%   Detailed explanation goes here
global N;
N = 7;

%% Initialise the variables
n = size(Sensor1_Target_Visit_Order, 2);
m = size(Sensor2_Target_Visit_Order, 2);

Scheduled_Targets = -1;
Status = 0;

if (n == 0) || (m == 0)
  % error in inputs, nothing to do.
  return;
end

%% Create the intervals and relations
intervals(1:n+m) = IA_Interval('', 0);
relations(1:n+m) = IA_Relation_Dummy();
for i = 1:n
  intervals(i) = IA_Interval(sprintf('S1_T%i', i), Sensor1_Target_Visit_Order(i));
  if (i ~= n)
    relations(i) = IA_Relation_Less();
  end
end
for i = 1:m
  intervals(i+n) = IA_Interval(sprintf('S2_T%i', i), Sensor2_Target_Visit_Order(i));
  if (i ~= m)
    relations(i+n) = IA_Relation_Less();
  end
end

ia_network = IA_Network(intervals(1), intervals(2), relations(1));
for i = 3:n+m
  ia_network = IA_Network_Add_Interval(ia_network, intervals(i));
  [ia_network, consistent] = IA_Network_Path_Consistency(ia_network, i-1, i, relations(i-1));
  if ~consistent
    % error in algorithm, should not happen if logic above has no bugs.
    return;
  end
end

%% Work out the "equals" relations and add new relations to the top priority bin in the Queue.
for i = 1:n
  for j = 1:m
    if (Sensor1_Target_Visit_Order(i) == Sensor2_Target_Visit_Order(j))
      found = 0;
      q_f = 0;
      t_f = 0;
      for q = 1:size(PQ, 2)
        for t = 1:size(PQ(q).tasks, 2)
          if PQ(q).tasks(t).target_id == Sensor1_Target_Visit_Order(i)
            found = 1;
            q_f = q;
            t_f = t;
            break;
          end
        end
        if found
          break;
        end
      end
      if ~found
        % add new task to the priority queue in bin 1
        PQ(1).tasks(size(PQ(1).tasks,2)+1) = struct('i', i, 'j', j, 'target_id', Sensor1_Target_Visit_Order(i));
      else
        % update the IA values for existent task
        PQ(q_f).tasks(t_f).i = i;
        PQ(q_f).tasks(t_f).j = j;
      end
    end
  end
end

%% Prune targets that have dissappeared (from either sensor)
% Is this necessary?
for q = 1:size(PQ, 2)
  for t = 1:size(PQ(q).tasks, 2)
    found = 0;
    for i = 1:n
      if PQ(q).tasks(t).target_id == Sensor1_Target_Visit_Order(i)
        found = 1;
        break;
      end
    end
    if found
      found = 0;
      for i = 1:m
        if PQ(q).tasks(t).target_id == Sensor2_Target_Visit_Order(i)
          found = 1;
          break;
        end
      end
    end
    if ~found
      % delete the task from the queue
      PQ(q).tasks = [PQ(q).tasks(1:t-1) PQ(q).tasks(t+1:size(Priority_Queue))];
    end
  end
end

%% Process the work list
k = 1;
SQ(1:size(PQ, 2)) = struct('tasks', struct('number', NaN, 'scheduled', 0));
for q = 1:size(PQ, 2)
  SQ(q).tasks(1:size(PQ(q).tasks, 2)) = struct('number', NaN, 'scheduled', 0);
  for t = 1:size(PQ(q).tasks, 2)
    % randomly select each task in the priority queue
    while 1
      SQ(q).tasks(t).number = ceil(rand()*(size(PQ(q).tasks, 2)));
      been_done = 0;
      for s = 1:t-1
        if SQ(q).tasks(s).number == SQ(q).tasks(t).number
          been_done = 1;
          break;
        end
      end
      if ~been_done
        break;
      end
    end
    s = SQ(q).tasks(t).number;
    i = PQ(q).tasks(s).i;
    j = PQ(q).tasks(s).j+n;
    [ia_temp_net, consistent] = IA_Network_Path_Consistency(ia_network, i, j, IA_Relation_Equals());
    if consistent
      ia_network = ia_temp_net;
      SQ(q).tasks(t).scheduled = 1;
      Scheduled_Targets(k) = PQ(q).tasks(s).target_id;
      k = k + 1;
    end
  end
end

temp = Scheduled_Targets;
k = 1;
for i = 1:size(Sensor1_Target_Visit_Order, 2)
  for j = 1:size(Scheduled_Targets, 2)
    if (Sensor1_Target_Visit_Order(i) == temp(j))
      Scheduled_Targets(k) = temp(j);
      k = k + 1;
    end
  end
end

%% Clean up the work list
for q = 1:size(SQ, 2)
  for t = 1:size(SQ(q).tasks, 2)
    s = SQ(q).tasks(t).number;
    if s <= 0
      continue;
    end
    if SQ(q).tasks(t).scheduled
      % remove from this priority bin and move to lower
      % as long as this is not the lowest bin already.
      if q ~= size(PQ, 2)
        temp_task = PQ(q).tasks(s);
        PQ(q).tasks = [PQ(q).tasks(1:s-1) PQ(q).tasks(s+1:size(PQ(q).tasks, 2))];
        PQ(q+1).tasks(size(PQ(q+1).tasks,2)+1) = temp_task;
        for u = t+1:size(SQ(q).tasks, 2)
          if SQ(q).tasks(u).number > SQ(q).tasks(t).number
            SQ(q).tasks(u).number = SQ(q).tasks(u).number - 1;
          end
        end
      end
    else
      % Move unscheduled tasks up in priority randomly.
      if (q ~= 1) && (rand() > (2/N))
        temp_task = PQ(q).tasks(s);
        PQ(q).tasks = [PQ(q).tasks(1:s-1) PQ(q).tasks(s+1:size(PQ(q).tasks, 2))];
        PQ(q-1).tasks(size(PQ(q-1).tasks,2)+1) = temp_task;
        for u = t+1:size(SQ(q).tasks, 2)
          if SQ(q).tasks(u).number > SQ(q).tasks(t).number
            SQ(q).tasks(u).number = SQ(q).tasks(u).number - 1;
          end
        end
      end
    end
  end
end
Status = 1;
end